import cv2
import numpy as np
from numpy import random
import math
from matplotlib import pyplot as plt
import matplotlib
from PIL import Image


# 计算旋转矩阵R
def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


def rotationMatrixToEulerAngles(R):
    assert (isRotationMatrix(R))

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])  # roll
        y = math.atan2(-R[2, 0], sy)  # pitch
        z = math.atan2(R[1, 0], R[0, 0])  # yaw
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])


def eulerAngles2rotationMat(theta, format='degree'):
    if format != 'degree':
        theta = [i * math.pi / 180.0 for i in theta]

    R_x = np.array([[1, 0, 0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0, math.sin(theta[0]), math.cos(theta[0])]
                    ])

    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0, 1, 0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]
                    ])

    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]), 0],
                    [0, 0, 1]
                    ])
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R


def calAngleFromRotation(R):
    # 绕Y转roll
    roll = np.arctan2(R[2, 1], R[2, 2]) * 180 / np.pi

    # 绕X转pitch
    pitch = np.arctan2(-R[2, 0], (R[0, 0] ** 2 + R[1, 0] ** 2) ** 0.5) * 180 / np.pi

    # 绕Z转yaw
    yaw = np.arctan2(R[1, 0], R[0, 0]) * 180 / np.pi
    return roll, pitch, yaw


if __name__ == '__main__':
    # 无误差
    a0list = []
    b0list = []
    c0list = []

    # 添加左相机内参标定误差
    a1list = []
    b1list = []
    c1list = []
    a1elist = []
    b1elist = []
    c1elist = []

    # 添加右相机内参标定误差
    a2list = []
    b2list = []
    c2list = []
    a2elist = []
    b2elist = []
    c2elist = []

    # 添加右相机到左相机外参误差
    a3list = []
    b3list = []
    c3list = []
    a3elist = []
    b3elist = []
    c3elist = []

    # 添加在左相机下待测点像素坐标误差
    a4list = []
    b4list = []
    c4list = []
    a4elist = []
    b4elist = []
    c4elist = []

    # 添加在右相机下待测点像素坐标误差
    a5list = []
    b5list = []
    c5list = []
    a5elist = []
    b5elist = []
    c5elist = []

    # 添加全误差
    a6list = []
    b6list = []
    c6list = []
    a6elist = []
    b6elist = []
    c6elist = []

    for i in range(1000):
        mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053, 2.71360000 / 0.0053])
        rp = np.array([5.40198700 / 0.0053, 2.71360000 / 0.0053])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        a0list.append(x)
        b0list.append(y)
        c0list.append(z)

    ######################################### 添加左相机标定误差 #########################################
    for i in range(1000):
        l_fx = random.uniform(0, 1)
        l_fy = random.uniform(0, 1)
        l_u0 = random.uniform(-0.5, 0.5)
        l_v0 = random.uniform(-0.5, 0.5)

        mtx_l = np.array([[100 / 0.0053 + l_fx, 0, 640 + l_u0, 0],
                          [0, 100 / 0.0053 + l_fy, 512 + l_v0, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053, 2.71360000 / 0.0053])
        rp = np.array([5.40198700 / 0.0053, 2.71360000 / 0.0053])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x1 = x[0]
        y1 = y[0]
        z1 = z[0]
        a1list.append(x1)
        b1list.append(y1)
        c1list.append(z1)
        a1elist.append(x1 - np.mean(a1list))
        b1elist.append(y1 - np.mean(b1list))
        c1elist.append(z1 - np.mean(c1list))

    a1 = a1list
    b1 = b1list
    c1 = c1list
    a11 = a1elist
    b11 = b1elist
    c11 = c1elist

    ######################################### 添加右相机标定误差 #########################################

    for i in range(1000):
        r_fx = random.uniform(0, 1)
        r_fy = random.uniform(0, 1)
        r_u0 = random.uniform(-0.5, 0.5)
        r_v0 = random.uniform(-0.5, 0.5)

        mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053 + r_fx, 0, 640 + r_u0, 0],
                          [0, 100 / 0.0053 + r_fy, 512 + r_v0, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053, 2.71360000 / 0.0053])
        rp = np.array([5.40198700 / 0.0053, 2.71360000 / 0.0053])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x2 = x[0]
        y2 = y[0]
        z2 = z[0]
        a2list.append(x2)
        b2list.append(y2)
        c2list.append(z2)
        a2elist.append(x2 - np.mean(a2list))
        b2elist.append(y2 - np.mean(b2list))

    ######################################### 添加右相机到左相机外参误差 #########################################
    for i in range(1000):
        R = random.uniform(-0.01, 0.01)
        P = random.uniform(-0.01, 0.01)
        Y = random.uniform(-0.01, 0.01)
        T_e = random.uniform(0, 1, size=(3, 1))
        mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])
        roll, pitch, yaw = calAngleFromRotation(R_lr)  # 弧度制
        roll0 = math.degrees(roll)
        pitch0 = math.degrees(pitch)
        yaw0 = math.degrees(yaw)
        roll_c = roll0 + R
        pitch_c = pitch0 + P
        yaw_c = yaw0 + Y
        R_lr = eulerAngles2rotationMat([roll_c * np.pi / 180, pitch_c * np.pi / 180, yaw_c * np.pi / 180])

        T_lr = T_lr + T_e

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053, 2.71360000 / 0.0053])
        rp = np.array([5.40198700 / 0.0053, 2.71360000 / 0.0053])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x3 = x[0]
        y3 = y[0]
        z3 = z[0]
        a3list.append(x3)
        b3list.append(y3)
        c3list.append(z3)
        a3elist.append(x3 - np.mean(a3list))
        b3elist.append(y3 - np.mean(b3list))
        c3elist.append(z3 - np.mean(c3list))

    ######################################### 添加在左相机待测点像素坐标误差 #########################################
    for i in range(1000):
        lp_e1 = random.uniform(-0.1, 0.1)
        lp_e2 = random.uniform(-0.1, 0.1)
        mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053 + lp_e1, 2.71360000 / 0.0053 + lp_e2])
        rp = np.array([5.40198700 / 0.0053, 2.71360000 / 0.0053])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x4 = x[0]
        y4 = y[0]
        z4 = z[0]
        a4list.append(x4)
        b4list.append(y4)
        c4list.append(z4)
        a4elist.append(x4 - np.mean(a4list))
        b4elist.append(y4 - np.mean(b4list))
        c4elist.append(z4 - np.mean(c4list))

    a4 = a4list
    b4 = b4list
    c4 = c4list

    a44 = a4elist
    b44 = b4elist
    c44 = c4elist

    ######################################### 添加在右相机待测点像素坐标误差 #########################################
    for i in range(1000):
        rp_e1 = random.uniform(-0.1, 0.1)
        rp_e2 = random.uniform(-0.1, 0.1)
        mtx_l = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[100 / 0.0053, 0, 640, 0],
                          [0, 100 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053, 2.71360000 / 0.0053])
        rp = np.array([5.40198700 / 0.0053 + rp_e1, 2.71360000 / 0.0053 + rp_e2])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x5 = x[0]
        y5 = y[0]
        z5 = z[0]
        a5list.append(x5)
        b5list.append(y5)
        c5list.append(z5)
        a5elist.append(x5 - np.mean(a5list))
        b5elist.append(y5 - np.mean(b5list))
        c5elist.append(z5 - np.mean(c5list))

    ######################################### 添加所有误差 #########################################
    for i in range(1000):
        # 左相机标定误差
        l_fx = random.uniform(-1, 1)
        l_fy = random.uniform(-1, 1)
        l_u0 = random.uniform(-0.5, 0.5)
        l_v0 = random.uniform(-0.5, 0.5)

        # 右相机标定误差
        r_fx = random.uniform(-1, 1)
        r_fy = random.uniform(-1, 1)
        r_u0 = random.uniform(-0.5, 0.5)
        r_v0 = random.uniform(-0.5, 0.5)

        # 两相机外参误差
        R = random.uniform(-0.01, 0.01)
        P = random.uniform(-0.01, 0.01)
        Y = random.uniform(-0.01, 0.01)
        T_e = random.uniform(-1, 1, size=(3, 1))

        # 左相机像素坐标误差
        lp_e1 = random.uniform(-0.1, 0.1)
        lp_e2 = random.uniform(-0.1, 0.1)

        # 右相机像素坐标误差
        rp_e1 = random.uniform(-0.1, 0.1)
        rp_e2 = random.uniform(-0.1, 0.1)

        mtx_l = np.array([[100 / 0.0053 + l_fx, 0, 640 + l_u0, 0],
                          [0, 100 / 0.0053 + l_fy, 512 + l_v0, 0],
                          [0, 0, 1, 0]])

        mtx_r = np.array([[100 / 0.0053 + r_fx, 0, 640 + r_u0, 0],
                          [0, 100 / 0.0053 + r_fy, 512 + r_v0, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[800],
                         [0],
                         [0]])

        roll, pitch, yaw = calAngleFromRotation(R_lr)  # 弧度制
        roll0 = math.degrees(roll)
        pitch0 = math.degrees(pitch)
        yaw0 = math.degrees(yaw)
        roll_c = roll0 + R
        pitch_c = pitch0 + P
        yaw_c = yaw0 + Y
        R_lr = eulerAngles2rotationMat([roll_c * np.pi / 180, pitch_c * np.pi / 180, yaw_c * np.pi / 180])

        T_lr = T_lr + T_e

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        Pl = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        Pr = np.dot(mtx_r, _temp_R_lr)

        # 像素坐标系下空间点坐标
        lp = np.array([1.38201300 / 0.0053 + lp_e1, 2.71360000 / 0.0053 + lp_e2])
        rp = np.array([5.40198700 / 0.0053 + rp_e1, 2.71360000 / 0.0053 + rp_e2])

        P = np.array(cv2.triangulatePoints(Pl, Pr, lp, rp))
        P /= P[3]
        x = P[0]
        y = P[1]
        z = P[2]
        x6 = x[0]
        y6 = y[0]
        z6 = z[0]
        a6list.append(x6)
        b6list.append(y6)
        c6list.append(z6)
        a6elist.append(x6 - np.mean(a6list))
        b6elist.append(y6 - np.mean(b6list))
        c6elist.append(z6 - np.mean(c6list))

    # 误差折线图
    fig = plt.figure()
    ax1 = fig.add_subplot(3, 1, 1)
    ax2 = fig.add_subplot(3, 1, 2)
    ax3 = fig.add_subplot(3, 1, 3)

    # 设置字体大小
    matplotlib.rcParams.update({'font.size': 10})

    x = np.linspace(0, 1000, 1000)

    z10 = a0list
    z11 = a1list
    z12 = a2list
    z13 = a3list
    z14 = a4list
    z15 = a5list
    z16 = a6list

    line10, = ax1.plot(x, z10, linewidth='1.5', color='k')
    line11, = ax1.plot(x, z11, linewidth='1', color='c')
    line12, = ax1.plot(x, z12, linewidth='1', color='g')
    line13, = ax1.plot(x, z13, linewidth='1', color='b')
    line14, = ax1.plot(x, z14, linewidth='1', color='m')
    line15, = ax1.plot(x, z15, linewidth='1', color='y')
    line16, = ax1.plot(x, z16, linewidth='1', color='r')

    ax1.set_xlabel('Point/pcs', fontsize=13)
    ax1.set_ylabel('x/mm', fontsize=13)

    z20 = b0list
    z21 = b1list
    z22 = b2list
    z23 = b3list
    z24 = b4list
    z25 = b5list
    z26 = b6list

    line20, = ax2.plot(x, z20, linewidth='1.5', color='k')
    line21, = ax2.plot(x, z21, linewidth='1', color='c')
    line22, = ax2.plot(x, z22, linewidth='1', color='g')
    line23, = ax2.plot(x, z23, linewidth='1', color='b')
    line24, = ax2.plot(x, z24, linewidth='1', color='m')
    line25, = ax2.plot(x, z25, linewidth='1', color='y')
    line26, = ax2.plot(x, z26, linewidth='1', color='r')

    ax2.set_xlabel('Point/pcs', fontsize=13)
    ax2.set_ylabel('y/mm', fontsize=13)

    z30 = c0list
    z31 = c1list
    z32 = c2list
    z33 = c3list
    z34 = c4list
    z35 = c5list
    z36 = c6list

    line30, = ax3.plot(x, z30, linewidth='1.5', color='k', label='Without \n error')
    line31, = ax3.plot(x, z31, linewidth='1', color='c', label='Internal \n parameter error \n of left camera')
    line32, = ax3.plot(x, z32, linewidth='1', color='g', label='Internal  \n parameter error \n of right camera')
    line33, = ax3.plot(x, z33, linewidth='1', color='b', label='External  \n parameter error \n of two camera')
    line34, = ax3.plot(x, z34, linewidth='1', color='m', label='Pixel coordinate \n in left camera')
    line35, = ax3.plot(x, z35, linewidth='1', color='y', label='Pixel coordinate \n in right camera')
    line36, = ax3.plot(x, z36, linewidth='1', color='r', label='Total errors')

    ax3.set_xlabel('Point/pcs', fontsize=13)
    ax3.set_ylabel('z/mm', fontsize=13)

    ax3.legend(bbox_to_anchor=(1, 0), loc=3, labelspacing=1, fontsize=11)

    plt.show()
